from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
from launch.conditions import IfCondition

def generate_launch_description():
    # 获取包路径
    go2_desc_pkg = get_package_share_directory("go2_description")
    go2_driver_pkg = get_package_share_directory("go2_driver_py")
    slam_toolbox_pkg = get_package_share_directory("slam_toolbox")
    lidar_transform_pkg = get_package_share_directory("go2_perception")
    go2_slam_pkg = get_package_share_directory("go2_agiros_slam")
    # slam_toolbox_config = os.path.join(go2_slam_pkg, 'config', 'mapper_params_online_async.yaml')

    # 设置配置文件路径
    slam_toolbox_config = os.path.join(
        get_package_share_directory('go2_agiros_slam'),
        'config',
        'mapper_params_online_async.yaml'
    )

    # 包含slam_toolbox的launch文件
    slam_toolbox_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            os.path.join(slam_toolbox_pkg, 'launch', 'online_async_launch.py')
        ]),
        launch_arguments={
            'params_file': slam_toolbox_config,
            'use_sim_time': 'false',
        }.items(),
    )

    # 声明参数
    use_rviz = DeclareLaunchArgument("use_rviz", default_value="true")

    return LaunchDescription([
        use_rviz,
        slam_toolbox_launch,
        
        # 1. 首先启动URDF模型和TF树
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                os.path.join(go2_desc_pkg, "launch", "display.launch.py")
            ),
            launch_arguments={"use_join_state_publisher": "false"}.items()
        ),
        
        # 2. 启动雷达点云转换（依赖TF树）
        IncludeLaunchDescription(
            PythonLaunchDescriptionSource(
                PathJoinSubstitution([lidar_transform_pkg, "launch", "go2_pointcloud_process.launch.py"])
            )
        ),
        
        # 3. 启动驱动节点（发布里程计）
        Node(
            package="go2_driver_py",
            executable="driver_2",
            parameters=[os.path.join(go2_driver_pkg, "params", "go2_driver.yaml")],
            output="screen"
        ),
        
        
        # 5. RViz可视化（最后启动）
        Node(
            package="rviz2",
            executable="rviz2",
            arguments=["-d", os.path.join(go2_slam_pkg, "rviz", "go2.rviz")],
            condition=IfCondition(LaunchConfiguration("use_rviz"))
        )
    ])